/**
  ******************************************************************************
  * @file stm32_hander.cpp
	*
	* The file for Dynamixel STM32 users.
	*
  * @author 健宝 <JohnLiuljj@163.com>
  ******************************************************************************
  */
#include "stm32_hander.h"
#include "main.h"
#include "port_handler.h"
#include "packet_handler.h"



using namespace dynamixel;



// Control table address
#define ADDR_OPERATING_MODE             11                  // Control table address is different in Dynamixel model
#define ADDR_TORQUE_ENABLE              64
#define ADDR_GOAL_POSITION              116									//0 - 4096
#define ADDR_GOAL_VELOCITY              104									//-330 - 330
#define ADDR_PRESENT_POSITION           132
#define ADDR_LED_ENABLE									65

// Protocol version
#define PROTOCOL_VERSION                2.0                 // See which protocol version is used in the Dynamixel

// Control mode
#define EXT_POSITION_CONTROL_MODE       3                   // Value for extended position control mode (operating mode)
#define EXT_VELOCITY_CONTROL_MODE       1                   // Value for extended velocity control mode (operating mode)

// Torque control
#define TORQUE_ENABLE                   1                   // Value for enabling the torque
#define TORQUE_DISABLE                  0                   // Value for disabling the torque

// LED control
#define LED_ENABLE            		      1                   // Value for enabling the torque
#define LED_DISABLE          	 		      0                   // Value for disabling the torque


////////////////////////////////////////////////////////////////////////////////
/// @brief The class for writing or reading Dynamixel data to contorl motors
////////////////////////////////////////////////////////////////////////////////
class STM32Drive
{
public:
  ////////////////////////////////////////////////////////////////////////////////
  /// @brief The function that Initializes instance for Stm32User
  /// @param port PortHandler instance
  /// @param ph PacketHandler instance
  ////////////////////////////////////////////////////////////////////////////////
  STM32Drive();

 
	void Ping(uint8_t id);
 
	void StartPositionControl(uint8_t id);
 
	void StartVelocityControl(uint8_t id);
 
	void StopControl(uint8_t id);

	void SetPosition(uint8_t id, uint32_t data);
	
	void SetVelocity(uint8_t id, uint32_t data);

  ////////////////////////////////////////////////////////////////////////////////
  /// @brief The function that calls clearParam function to clear the parameter list for Sync WriteclearParam();
  ////////////////////////////////////////////////////////////////////////////////
  ~STM32Drive() {}


private:

	
	uint8_t	ifTorque = 1;
	uint8_t controlMode = 0;
  PortHandler *portHandler;
  PacketHandler *packetHandler;
};





STM32Drive::STM32Drive()
{
	portHandler = PortHandler::getPortHandler("STM32Drive");
	packetHandler = PacketHandler::getPacketHandler(2.0);
}


void STM32Drive::Ping(uint8_t id){
	int dxl_comm_result = COMM_TX_FAIL;             // Communication result
	uint16_t dxl_model_number;                      // Dynamixel model number
	uint8_t dxl_error = 0;                          // Dynamixel error
	dxl_comm_result = COMM_TX_FAIL;
	while(dxl_comm_result != COMM_SUCCESS){
		dxl_comm_result = packetHandler->ping(portHandler, id, &dxl_model_number, &dxl_error);
	}
}


void STM32Drive::StopControl(uint8_t id){
	int dxl_comm_result = COMM_TX_FAIL;             // Communication result
	uint8_t dxl_error = 0;                          // Dynamixel error
	
	if(ifTorque == 1){
		while(dxl_comm_result != COMM_SUCCESS){
			dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_TORQUE_ENABLE, TORQUE_DISABLE, &dxl_error);
		}
		ifTorque = 0;
	}
	controlMode = 0;
}


void STM32Drive::StartPositionControl(uint8_t id){
	int dxl_comm_result = COMM_TX_FAIL;             // Communication result
	uint8_t dxl_error = 0;                          // Dynamixel error

	StopControl(id);
	
	dxl_comm_result = COMM_TX_FAIL;
	while(dxl_comm_result != COMM_SUCCESS){
		dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_OPERATING_MODE, EXT_POSITION_CONTROL_MODE, &dxl_error);
	}
	controlMode = EXT_POSITION_CONTROL_MODE;
	// Enable Dynamixel Torque
	dxl_comm_result = COMM_TX_FAIL;
	while(dxl_comm_result != COMM_SUCCESS){
		dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
	}
	ifTorque = 1;
}


void STM32Drive::StartVelocityControl(uint8_t id){
	int dxl_comm_result = COMM_TX_FAIL;             // Communication result
	uint8_t dxl_error = 0;                          // Dynamixel error

	StopControl(id);
	
	dxl_comm_result = COMM_TX_FAIL;
	while(dxl_comm_result != COMM_SUCCESS){
		dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_OPERATING_MODE, EXT_VELOCITY_CONTROL_MODE, &dxl_error);
	}
	controlMode = EXT_VELOCITY_CONTROL_MODE;
	
	// Enable Dynamixel Torque
	dxl_comm_result = COMM_TX_FAIL;
	while(dxl_comm_result != COMM_SUCCESS){
		dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, ADDR_TORQUE_ENABLE, TORQUE_ENABLE, &dxl_error);
	}
	ifTorque = 1;
}


void STM32Drive::SetPosition(uint8_t id, uint32_t data){
	int dxl_comm_result = COMM_TX_FAIL;             // Communication result
	uint8_t dxl_error = 0;                          // Dynamixel error
	
	if(controlMode != EXT_POSITION_CONTROL_MODE){
		StartPositionControl(id);
	}
	
	while(dxl_comm_result != COMM_SUCCESS){
		dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, id, ADDR_GOAL_POSITION, data, &dxl_error);
	}
}


void STM32Drive::SetVelocity(uint8_t id, uint32_t data){
	int dxl_comm_result = COMM_TX_FAIL;             // Communication result
	uint8_t dxl_error = 0;                          // Dynamixel error
	
	if(controlMode != EXT_VELOCITY_CONTROL_MODE){
		StartVelocityControl(id);
	}
	
	while(dxl_comm_result != COMM_SUCCESS){
		dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, id, ADDR_GOAL_VELOCITY, data, &dxl_error);
	}
}




void DynamixelTest(void){
	
	static STM32Drive *stm3232Drive;
	static uint8_t i = 0;
	uint8_t DXL_ID = 3;

	if(i == 0){
		stm3232Drive = (new STM32Drive());
		i = 1;
	}
	
	stm3232Drive->SetVelocity(DXL_ID, 50);
	HAL_Delay(3000);
	stm3232Drive->SetVelocity(DXL_ID, 200);
	HAL_Delay(3000);
	stm3232Drive->SetPosition(DXL_ID, 1000);
	HAL_Delay(3000);
	stm3232Drive->SetPosition(DXL_ID, 4000);
	HAL_Delay(3000);
}

